// This file is based on material originally from:
// Geometric Tools, LLC
// Copyright (c) 1998-2010
// Distributed under the Boost Software License, Version 1.0.
// http://www.boost.org/LICENSE_1_0.txt
// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt


#ifndef __Quaternion_H__
#define __Quaternion_H__

#include "U2PreRequest.h"
#include "U2Math.h"
#include "U2Vector3.h"
#include "U2Matrix3.h"


U2EG_NAMESPACE_BEGIN


/** Implementation of a U2Quaternion, i.e. a rotation around an axis.
	For more information about Quaternions and the theory behind it, we recommend reading:
	http://www.ogre3d.org/tikiwiki/U2Quaternion+and+Rotation+Primer
	http://www.cprogramming.com/tutorial/3d/quaternions.html
	http://www.gamedev.net/page/resources/_/reference/programming/math-and-physics/
	quaternions/quaternion-powers-r1095
*/
class _U2Share U2Quaternion
{
public:
	/// Default constructor, initializes to identity rotation (aka 0°)
	inline U2Quaternion ()
		: w(1), x(0), y(0), z(0)
	{
	}
	/// Construct from an explicit list of values
	inline U2Quaternion (
		u2real fW,
		u2real fX, u2real fY, u2real fZ)
		: w(fW), x(fX), y(fY), z(fZ)
	{
	}
    /// Construct a quaternion from a rotation matrix
    inline U2Quaternion(const U2Matrix3& rot)
    {
        this->FromRotationMatrix(rot);
    }
    /// Construct a quaternion from an angle/axis
    inline U2Quaternion(const Radian& rfAngle, const U2Vector3& rkAxis)
    {
        this->FromAngleAxis(rfAngle, rkAxis);
    }
    /// Construct a quaternion from 3 orthonormal local axes
    inline U2Quaternion(const U2Vector3& xaxis, const U2Vector3& yaxis, const U2Vector3& zaxis)
    {
        this->FromAxes(xaxis, yaxis, zaxis);
    }
    /// Construct a quaternion from 3 orthonormal local axes
    inline U2Quaternion(const U2Vector3* akAxis)
    {
        this->FromAxes(akAxis);
    }
	/// Construct a quaternion from 4 manual w/x/y/z values
	inline U2Quaternion(u2real* valptr)
	{
		memcpy(&w, valptr, sizeof(u2real)*4);
	}

	/** Exchange the contents of this quaternion with another. 
	*/
	inline void swap(U2Quaternion& other)
	{
		std::swap(w, other.w);
		std::swap(x, other.x);
		std::swap(y, other.y);
		std::swap(z, other.z);
	}

	/// Array accessor operator
	inline u2real operator [] ( const size_t i ) const
	{
		assert( i < 4 );

		return *(&w+i);
	}

	/// Array accessor operator
	inline u2real& operator [] ( const size_t i )
	{
		assert( i < 4 );

		return *(&w+i);
	}

	/// Pointer accessor for direct copying
	inline u2real* ptr()
	{
		return &w;
	}

	/// Pointer accessor for direct copying
	inline const u2real* ptr() const
	{
		return &w;
	}

	void FromRotationMatrix (const U2Matrix3& kRot);
    void ToRotationMatrix (U2Matrix3& kRot) const;
	/** Setups the quaternion using the supplied vector, and "roll" around
		that vector by the specified radians.
	*/
    void FromAngleAxis (const Radian& rfAngle, const U2Vector3& rkAxis);
    void ToAngleAxis (Radian& rfAngle, U2Vector3& rkAxis) const;
    inline void ToAngleAxis (Degree& dAngle, U2Vector3& rkAxis) const {
        Radian rAngle;
        ToAngleAxis ( rAngle, rkAxis );
        dAngle = rAngle;
    }
	/** Constructs the quaternion using 3 axes, the axes are assumed to be orthonormal
		@See FromAxes
	*/
    void FromAxes (const U2Vector3* akAxis);
    void FromAxes (const U2Vector3& xAxis, const U2Vector3& yAxis, const U2Vector3& zAxis);
	/** Gets the 3 orthonormal axes defining the quaternion. @See FromAxes */
    void ToAxes (U2Vector3* akAxis) const;
    void ToAxes (U2Vector3& xAxis, U2Vector3& yAxis, U2Vector3& zAxis) const;

	/** Returns the X orthonormal axis defining the quaternion. Same as doing
		xAxis = U2Vector3::UNIT_X * this. Also called the local X-axis
	*/
    U2Vector3 xAxis(void) const;

    /** Returns the Y orthonormal axis defining the quaternion. Same as doing
		yAxis = U2Vector3::UNIT_Y * this. Also called the local Y-axis
	*/
    U2Vector3 yAxis(void) const;

	/** Returns the Z orthonormal axis defining the quaternion. Same as doing
		zAxis = U2Vector3::UNIT_Z * this. Also called the local Z-axis
	*/
    U2Vector3 zAxis(void) const;

    inline U2Quaternion& operator= (const U2Quaternion& rkQ)
	{
		w = rkQ.w;
		x = rkQ.x;
		y = rkQ.y;
		z = rkQ.z;
		return *this;
	}
    U2Quaternion operator+ (const U2Quaternion& rkQ) const;
    U2Quaternion operator- (const U2Quaternion& rkQ) const;
    U2Quaternion operator* (const U2Quaternion& rkQ) const;
    U2Quaternion operator* (u2real fScalar) const;

    inline _U2Share friend U2Quaternion operator* (u2real fScalar,
        const U2Quaternion& rkQ)
    {
        return U2Quaternion(fScalar*rkQ.w,fScalar*rkQ.x,fScalar*rkQ.y,
            fScalar*rkQ.z);
    }

    U2Quaternion operator- () const;
    inline bool operator== (const U2Quaternion& rhs) const
	{
		return (rhs.x == x) && (rhs.y == y) &&
			(rhs.z == z) && (rhs.w == w);
	}
    inline bool operator!= (const U2Quaternion& rhs) const
	{
		return !operator==(rhs);
	}
    // functions of a quaternion
    u2real Dot (const U2Quaternion& rkQ) const;  // dot product
    u2real Norm () const;  // squared-length
    /// Normalises this quaternion, and returns the previous length
    u2real normalise(void); 
    U2Quaternion Inverse () const;  // apply to non-zero quaternion
    U2Quaternion UnitInverse () const;  // apply to unit-length quaternion
    U2Quaternion Exp () const;
    U2Quaternion Log () const;

    // rotation of a vector by a quaternion
    U2Vector3 operator* (const U2Vector3& rkVector) const;

	/** Calculate the local roll element of this quaternion.
	@param reprojectAxis By default the method returns the 'intuitive' result
		that is, if you projected the local Y of the quaternion onto the X and
		Y axes, the angle between them is returned. If set to false though, the
		result is the actual yaw that will be used to implement the quaternion,
		which is the shortest possible path to get to the same orientation and 
		may involve less axial rotation. 
	*/
	Radian getRoll(bool reprojectAxis = true) const;
	/** Calculate the local pitch element of this quaternion
	@param reprojectAxis By default the method returns the 'intuitive' result
		that is, if you projected the local Z of the quaternion onto the X and
		Y axes, the angle between them is returned. If set to true though, the
		result is the actual yaw that will be used to implement the quaternion,
		which is the shortest possible path to get to the same orientation and 
		may involve less axial rotation. 
	*/
	Radian getPitch(bool reprojectAxis = true) const;
	/** Calculate the local yaw element of this quaternion
	@param reprojectAxis By default the method returns the 'intuitive' result
		that is, if you projected the local Z of the quaternion onto the X and
		Z axes, the angle between them is returned. If set to true though, the
		result is the actual yaw that will be used to implement the quaternion,
		which is the shortest possible path to get to the same orientation and 
		may involve less axial rotation. 
	*/
	Radian getYaw(bool reprojectAxis = true) const;		
	/// Equality with tolerance (tolerance is max angle difference)
	bool equals(const U2Quaternion& rhs, const Radian& tolerance) const;
	
    /** Performs Spherical linear interpolation between two quaternions, and returns the result.
		Slerp ( 0.0f, A, B ) = A
		Slerp ( 1.0f, A, B ) = B
		@returns Interpolated quaternion
		@remarks
		Slerp has the proprieties of performing the interpolation at constant
		velocity, and being torque-minimal (unless shortestPath=false).
		However, it's NOT commutative, which means
		Slerp ( 0.75f, A, B ) != Slerp ( 0.25f, B, A );
		therefore be careful if your code relies in the order of the operands.
		This is specially important in IK animation.
	*/
    static U2Quaternion Slerp (u2real fT, const U2Quaternion& rkP,
        const U2Quaternion& rkQ, bool shortestPath = false);

	/** @See Slerp. It adds extra "spins" (i.e. rotates several times) specified
		by parameter 'iExtraSpins' while interpolating before arriving to the
		final values
	*/
    static U2Quaternion SlerpExtraSpins (u2real fT,
        const U2Quaternion& rkP, const U2Quaternion& rkQ,
        int iExtraSpins);

    // setup for spherical quadratic interpolation
    static void Intermediate (const U2Quaternion& rkQ0,
        const U2Quaternion& rkQ1, const U2Quaternion& rkQ2,
        U2Quaternion& rka, U2Quaternion& rkB);

    // spherical quadratic interpolation
    static U2Quaternion Squad (u2real fT, const U2Quaternion& rkP,
        const U2Quaternion& rkA, const U2Quaternion& rkB,
        const U2Quaternion& rkQ, bool shortestPath = false);

    /** Performs Normalised linear interpolation between two quaternions, and returns the result.
		nlerp ( 0.0f, A, B ) = A
		nlerp ( 1.0f, A, B ) = B
		@remarks
		Nlerp is faster than Slerp.
		Nlerp has the proprieties of being commutative (@See Slerp;
		commutativity is desired in certain places, like IK animation), and
		being torque-minimal (unless shortestPath=false). However, it's performing
		the interpolation at non-constant velocity; sometimes this is desired,
		sometimes it is not. Having a non-constant velocity can produce a more
		natural rotation feeling without the need of tweaking the weights; however
		if your scene relies on the timing of the rotation or assumes it will point
		at a specific angle at a specific weight value, Slerp is a better choice.
	*/
    static U2Quaternion nlerp(u2real fT, const U2Quaternion& rkP, 
        const U2Quaternion& rkQ, bool shortestPath = false);

    // cutoff for sine near zero
    static const u2real msEpsilon;

    // special values
    static const U2Quaternion ZERO;
    static const U2Quaternion IDENTITY;

	u2real w, x, y, z;

	/// Check whether this quaternion contains valid values
	inline bool isNaN() const
	{
		return U2Math::isNaN(x) || U2Math::isNaN(y) || U2Math::isNaN(z) || U2Math::isNaN(w);
	}

    /** Function for writing to a stream. Outputs "U2Quaternion(w, x, y, z)" with w,x,y,z
        being the member values of the quaternion.
    */
    inline _U2Share friend std::ostream& operator <<
        ( std::ostream& o, const U2Quaternion& q )
    {
        o << "U2Quaternion(" << q.w << ", " << q.x << ", " << q.y << ", " << q.z << ")";
        return o;
    }

};


U2EG_NAMESPACE_END


#endif 
